//
// Created by Xiezongxi on 2025/6/7.
//
#include "drv_kalman.h"
//1. 结构体类型定义

//Q和R需要自己去调
// typedef struct
// {
//     float LastP;//上次估算协方差 初始化值为0.02
//     float Now_P;//当前估算协方差 初始化值为0
//     float out;//卡尔曼滤波器输出 初始化值为0
//     float Kg;//卡尔曼增益 初始化值为0
//     float Q;//过程噪声协方差 初始化值为0.001
//     float R;//观测噪声协方差 初始化值为0.543
// }KFP;//Kalman Filter parameter

//2. 以高度为例 定义卡尔曼结构体并初始化参数
// KFP KFP_Mapan_x={0.02,0,0,0,0.001,0.543};
// KFP KFP_Mapan_y={0.02,0,0,0,0.001,0.543};
// KFP KFP_Mid360_x={0.02,0,0,0,0.001,0.543};
// KFP KFP_Mid360_y={0.02,0,0,0,0.001,0.543};


/**
 *卡尔曼滤波器
 *@param KFP *kfp 卡尔曼结构体参数
 *   float input 需要滤波的参数的测量值（即传感器的采集值）
 *@return 滤波后的参数（最优值）
 */
float kalmanFilter(KFP *kfp,float input)
{
    //预测协方差方程：k时刻系统估算协方差 = k-1时刻的系统协方差 + 过程噪声协方差
    kfp->Now_P = kfp->LastP + kfp->Q;
    //卡尔曼增益方程：卡尔曼增益 = k时刻系统估算协方差 / （k时刻系统估算协方差 + 观测噪声协方差）
    kfp->Kg = kfp->Now_P / (kfp->Now_P + kfp->R);
    //更新最优值方程：k时刻状态变量的最优值 = 状态变量的预测值 + 卡尔曼增益 * （测量值 - 状态变量的预测值）
    kfp->out = kfp->out + kfp->Kg * (input -kfp->out);//因为这一次的预测值就是上一次的输出值
    //更新协方差方程: 本次的系统协方差付给 kfp->LastP 威下一次运算准备。
    kfp->LastP = (1-kfp->Kg) * kfp->Now_P;
    return kfp->out;
}

float kalmanFilter_Merge(KFP *kfp1,KFP *kfp2,float input1,float input2)
{
    //预测协方差方程：k时刻系统估算协方差 = k-1时刻的系统协方差 + 过程噪声协方差
    kfp1->Now_P = kfp1->LastP + kfp1->Q;
    //卡尔曼增益方程：卡尔曼增益 = k时刻系统估算协方差 / （k时刻系统估算协方差 + 观测噪声协方差）
    kfp1->Kg = kfp1->Now_P / (kfp1->Now_P + kfp1->R);
    //更新最优值方程：k时刻状态变量的最优值 = 状态变量的预测值 + 卡尔曼增益 * （测量值 - 状态变量的预测值）
    kfp1->out = kfp1->out + kfp1->Kg * (input1 -kfp1->out);//因为这一次的预测值就是上一次的输出值
    //更新协方差方程: 本次的系统协方差付给 kfp1->LastP 威下一次运算准备。
    kfp1->LastP = (1-kfp1->Kg) * kfp1->Now_P;

    //预测协方差方程：k时刻系统估算协方差 = k-1时刻的系统协方差 + 过程噪声协方差
    kfp2->Now_P = kfp2->LastP + kfp2->Q;
    //卡尔曼增益方程：卡尔曼增益 = k时刻系统估算协方差 / （k时刻系统估算协方差 + 观测噪声协方差）
    kfp2->Kg = kfp2->Now_P / (kfp2->Now_P + kfp2->R);
    //更新最优值方程：k时刻状态变量的最优值 = 状态变量的预测值 + 卡尔曼增益 * （测量值 - 状态变量的预测值）
    kfp2->out = kfp2->out + kfp2->Kg * (input2 -kfp2->out);//因为这一次的预测值就是上一次的输出值
    //更新协方差方程: 本次的系统协方差付给 kfp2->LastP 威下一次运算准备。
    kfp2->LastP = (1-kfp2->Kg) * kfp2->Now_P;

    return (kfp1->Kg*kfp1->out + kfp2->Kg*kfp2->out)/(kfp1->Kg + kfp2->Kg);
}

// 估计 R 和 Q
void estimateNoiseParams(KFP *kfp, float *measurements, int n) {
    float total_residual = 0.0;
    float total_process_variation = 0.0;
    
    // 模拟通过一段时间的测量估计 R 和 Q
    for (int i = 0; i < n; i++) {
        float z = measurements[i];  // 获取测量值
        
        kalmanFilter(kfp,z);

        // 计算残差 (测量值 - 预测值)
        float residual = z - kfp->out;
        total_residual += residual * residual;
        
        // 估计过程变化（预测值和真实值的差异）
        total_process_variation += kfp->LastP;
    }

    // 基于残差估计 R
    kfp->R = total_residual / n;
    
    // 基于过程变化估计 Q
    kfp->Q = total_process_variation / n;
}
